#include "plan_env/grid_map.h"
#include "time.h"

// #define current_img_ md_.depth_image_[image_cnt_ & 1]
// #define last_img_ md_.depth_image_[!(image_cnt_ & 1)]

clock_t start_clock,end_clock;

void GridMap::initMap(ros::NodeHandle &nh)
{
  node_ = nh;

  /* get parameter */
  double x_size, y_size, z_size;
  node_.param("grid_map/resolution", mp_.resolution_, -1.0);
  node_.param("grid_map/map_size_x", x_size, -1.0);
  node_.param("grid_map/map_size_y", y_size, -1.0);
  node_.param("grid_map/map_size_z", z_size, -1.0);
  node_.param("grid_map/local_update_range_x", mp_.local_update_range_(0), -1.0);
  node_.param("grid_map/local_update_range_y", mp_.local_update_range_(1), -1.0);
  node_.param("grid_map/local_update_range_z", mp_.local_update_range_(2), -1.0);
  node_.param("grid_map/obstacles_inflation", mp_.obstacles_inflation_, -1.0);
  node_.param("grid_map/obstacles_inflation2d", mp_.obstacles_inflation2d_, -1.0);

  node_.param("grid_map/p_hit", mp_.p_hit_, 0.70);
  node_.param("grid_map/p_miss", mp_.p_miss_, 0.35);
  node_.param("grid_map/p_min", mp_.p_min_, 0.12);
  node_.param("grid_map/p_max", mp_.p_max_, 0.97);
  node_.param("grid_map/p_occ", mp_.p_occ_, 0.80);
  node_.param("grid_map/min_ray_length", mp_.min_ray_length_, -0.1);
  node_.param("grid_map/max_ray_length", mp_.max_ray_length_, -0.1);

  node_.param("grid_map/frame_id", mp_.frame_id_, string("map"));
  node_.param("grid_map/ground_height", mp_.ground_height_, 1.0);
  node_.param("grid_map/decay_time_threshold", md_.decay_time_threshold_, 1.0);
  node_.param("grid_map/decay_factor", md_.decay_factor_, 0.99);
  
  node_.param("grid_map/lidar_need_to_body", mp_.lidar_need_to_body_, false);
  node_.param("grid_map/lidar_need_to_world", mp_.lidar_need_to_world_, false);

  mp_.resolution_inv_ = 1 / mp_.resolution_;
  mp_.map_origin_ = Eigen::Vector3d(-x_size / 2.0, -y_size / 2.0, mp_.ground_height_);
  mp_.map_size_ = Eigen::Vector3d(x_size, y_size, z_size);

  mp_.prob_hit_log_ = logit(mp_.p_hit_);
  mp_.prob_miss_log_ = logit(mp_.p_miss_);
  mp_.clamp_min_log_ = logit(mp_.p_min_);
  mp_.clamp_max_log_ = logit(mp_.p_max_);
  mp_.min_occupancy_log_ = logit(mp_.p_occ_);
  mp_.unknown_flag_ = 0.01;

  cout << "hit: " << mp_.prob_hit_log_ << endl;
  cout << "miss: " << mp_.prob_miss_log_ << endl;
  cout << "min log: " << mp_.clamp_min_log_ << endl;
  cout << "max: " << mp_.clamp_max_log_ << endl;
  cout << "thresh log: " << mp_.min_occupancy_log_ << endl;

  for (int i = 0; i < 3; ++i)
    mp_.map_voxel_num_(i) = ceil(mp_.map_size_(i) / mp_.resolution_);

  mp_.map_min_boundary_ = mp_.map_origin_;
  mp_.map_max_boundary_ = mp_.map_origin_ + mp_.map_size_;

  // initialize data buffers
  mp_.buffer_size_ = mp_.map_voxel_num_(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2);
  mp_.buffer_size_2d_ = mp_.map_voxel_num_(0) * mp_.map_voxel_num_(1);

  md_.occupancy_buffer_ = vector<double>(mp_.buffer_size_, mp_.clamp_min_log_ - mp_.unknown_flag_);
  md_.occupancy_buffer_inflate_ = vector<char>(mp_.buffer_size_, 0);
  md_.last_update_time_buffer_ = vector<double>(mp_.buffer_size_, ros::Time::now().toSec());

  md_.occupancy_buffer_inflate_2d_ = vector<char>(mp_.buffer_size_2d_,0);

  md_.count_hit_and_miss_ = vector<short>(mp_.buffer_size_, 0);
  md_.count_hit_ = vector<short>(mp_.buffer_size_, 0);
  md_.count_miss_ = vector<short>(mp_.buffer_size_, 0);

  /* init callback */

  // use odometry and point cloud
  indep_cloud_sub_ =
      node_.subscribe<sensor_msgs::PointCloud2>("/grid_map/cloud", 10, &GridMap::cloudCallback, this);
  indep_odom_sub_ =
      node_.subscribe<nav_msgs::Odometry>("/grid_map/odom", 10, &GridMap::odomCallback, this);

  vis_timer_ = node_.createTimer(ros::Duration(0.5), &GridMap::visCallback, this);
  time_decay_timer_ = node_.createTimer(ros::Duration(0.2), &GridMap::TimeDecayCallback, this);

  map_2d_pub_ = node_.advertise<nav_msgs::OccupancyGrid>("/map_2d",10);
  map_pub_ = node_.advertise<sensor_msgs::PointCloud2>("/grid_map/occupancy", 10);
  map_inf_pub_ = node_.advertise<sensor_msgs::PointCloud2>("/grid_map/occupancy_inflate", 10);

  map_local_2D_pub_ = node_.advertise<sensor_msgs::PointCloud2>("/sdf_map/occupancy_local_inflate_2D", 10);
  map_local_pub_ = node_.advertise<sensor_msgs::PointCloud2>("/sdf_map/occupancy_local", 10);
  map_local_inflate_pub_ = node_.advertise<sensor_msgs::PointCloud2>("/sdf_map/occupancy_local_inflate", 10);

  unknown_pub_ = node_.advertise<sensor_msgs::PointCloud2>("/grid_map/unknown", 10);
  terrain_height_pub_ = node_.advertise<sensor_msgs::PointCloud2>("/grid_map/terrain_height", 10);

  box_pub_ = node_.advertise<visualization_msgs::Marker>("/sdf_map/map_explore_box", 10);

  md_.has_odom_ = false;
  md_.has_cloud_ = false;

  // rand_noise_ = uniform_real_distribution<double>(-0.2, 0.2);
  // rand_noise2_ = normal_distribution<double>(0, 0.2);
  // random_device rd;
  // eng_ = default_random_engine(rd());
}

void GridMap::resetBuffer()
{
  Eigen::Vector3d min_pos = mp_.map_min_boundary_;
  Eigen::Vector3d max_pos = mp_.map_max_boundary_;

  resetBuffer(min_pos, max_pos);

  md_.local_bound_min_ = Eigen::Vector3i::Zero();
  md_.local_bound_max_ = mp_.map_voxel_num_ - Eigen::Vector3i::Ones();
}

void GridMap::resetBuffer(Eigen::Vector3d min_pos, Eigen::Vector3d max_pos)
{

  Eigen::Vector3i min_id, max_id;
  posToIndex(min_pos, min_id);
  posToIndex(max_pos, max_id);

  boundIndex(min_id);
  boundIndex(max_id);

  /* reset occ and dist buffer */
  for (int x = min_id(0); x <= max_id(0); ++x)
    for (int y = min_id(1); y <= max_id(1); ++y)
      for (int z = min_id(2); z <= max_id(2); ++z)
      {
        md_.occupancy_buffer_inflate_[toAddress(x, y, z)] = 0;
      }
}

//记录grid_map中每一个栅格被ray穿过的次数，并记录第一个次被穿过的元素到cache_voxel_中
int GridMap::setCacheOccupancy(Eigen::Vector3d pos, int occ)
{
  if (occ != 1 && occ != 0)
    return INVALID_IDX;

  Eigen::Vector3i id;
  posToIndex(pos, id);
  int idx_ctns = toAddress(id);

  md_.count_hit_and_miss_[idx_ctns] += 1;

  if (md_.count_hit_and_miss_[idx_ctns] == 1)
  {
    md_.cache_voxel_.push(id);
  }

  if (occ == 1)
    md_.count_hit_[idx_ctns] += 1;

  return idx_ctns;
}

void GridMap::SetCacheOccupancy(const int& adr, const int& occ) {
  // Add to update list if first visited
  if (md_.count_hit_[adr] == 0 && md_.count_miss_[adr] == 0) md_.cache_voxel_int_.push(adr);

  if (occ == 0)
    md_.count_miss_[adr] = 1;
  else /* if (occ == 1) */
    md_.count_hit_[adr] += 1;

  // md_->count_hit_and_miss_[adr] += 1;
  // if (occ == 1)
  //   md_->count_hit_[adr] += 1;
  // if (md_->count_hit_and_miss_[adr] == 1)
  //   md_->cache_voxel_.push(adr);
}

Eigen::Vector3d GridMap::closetPointInMap(const Eigen::Vector3d &pt, const Eigen::Vector3d &camera_pt)
{
  Eigen::Vector3d diff = pt - camera_pt;
  Eigen::Vector3d max_tc = mp_.map_max_boundary_ - camera_pt;
  Eigen::Vector3d min_tc = mp_.map_min_boundary_ - camera_pt;

  double min_t = 1000000;

  for (int i = 0; i < 3; ++i)
  {
    if (fabs(diff[i]) > 0)
    {

      double t1 = max_tc[i] / diff[i];
      if (t1 > 0 && t1 < min_t)
        min_t = t1;

      double t2 = min_tc[i] / diff[i];
      if (t2 > 0 && t2 < min_t)
        min_t = t2;
    }
  }

  return camera_pt + (min_t - 1e-3) * diff;
}

void GridMap::visCallback(const ros::TimerEvent & /*event*/) {
  PublishVisualization(); // 发布占据栅格，膨胀栅格，2D膨胀栅格的可视化
  PublishMapBoxAndLocalUpdateBox(); // 发布map及local update box的可视化
}
void GridMap::odomCallback(const nav_msgs::OdometryConstPtr &odom) {

  md_.camera_pos_(0) = odom->pose.pose.position.x;
  md_.camera_pos_(1) = odom->pose.pose.position.y;
  md_.camera_pos_(2) = odom->pose.pose.position.z;
  
  if (!md_.has_odom_) {
    // 首次更新odom，也意味着程序初始化完成
    md_.has_odom_ = true;
    // 直接根据当前定位进行一次地图原点的更新
    ChangeMapOrigin(md_.camera_pos_);
  }
  // 更新局部local
  posToIndex(md_.camera_pos_ + mp_.local_update_range_, md_.local_bound_max_);
  posToIndex(md_.camera_pos_ - mp_.local_update_range_, md_.local_bound_min_);
  boundIndex(md_.local_bound_min_);
  boundIndex(md_.local_bound_max_);
  // 如果需要更新转换矩阵
  if (mp_.lidar_need_to_world_) {
    // 提取四元数表示的姿态信息
    Eigen::Quaterniond odom_q = Eigen::Quaterniond(
      odom->pose.pose.orientation.w,
      odom->pose.pose.orientation.x,
      odom->pose.pose.orientation.y,
      odom->pose.pose.orientation.z);

    // 将四元数转换为旋转矩阵
    Eigen::Matrix3d odom_r_m = odom_q.toRotationMatrix();

    // 构建完整的转换矩阵（从机器人坐标系到世界坐标系）
    md_.Tw_b_ = Eigen::Matrix4d::Identity();
    md_.Tw_b_.block<3, 3>(0, 0) = odom_r_m;
    md_.Tw_b_(0, 3) = md_.camera_pos_(0);
    md_.Tw_b_(1, 3) = md_.camera_pos_(1);
    md_.Tw_b_(2, 3) = md_.camera_pos_(2);
  }
}

void GridMap::cloudCallback(const sensor_msgs::PointCloud2ConstPtr &img) {
  pcl::PointCloud<pcl::PointXYZ> latest_cloud;
  pcl::fromROSMsg(*img, latest_cloud);
  int cloud_num = latest_cloud.points.size();

  md_.has_cloud_ = true;

  if (!md_.has_odom_) {
    std::cout << "no odom!" << std::endl;
    return;
  }

  if (latest_cloud.points.size() == 0)
    return;

  if (isnan(md_.camera_pos_(0)) || isnan(md_.camera_pos_(1)) || isnan(md_.camera_pos_(2)))
    return;
  // 激光系下点云转body系
  // if (mp_.lidar_need_to_body_) {

  // }
  // body系下点云转到世界系下
  if (mp_.lidar_need_to_world_) {
    pcl::PointCloud<pcl::PointXYZ> world_cloud;
    pcl::transformPointCloud(latest_cloud, world_cloud, md_.Tw_b_);
    latest_cloud = world_cloud;
  }
  // 需要世界系下的点云
  inputPointCloud(latest_cloud, cloud_num, md_.camera_pos_);

  return;
}

void GridMap::publishMap2D() {
    nav_msgs::OccupancyGrid map_2d;
    map_2d.header.frame_id = "map";
    map_2d.info.resolution = mp_.resolution_;
    map_2d.info.width = md_.local_bound_max_(0)-md_.local_bound_min_(0);
    map_2d.info.height = md_.local_bound_max_(1)-md_.local_bound_min_(1);
    map_2d.data = std::vector<int8_t>(map_2d.info.width*map_2d.info.height,0);
    Eigen::Vector3d origin;
    indexToPos(md_.local_bound_min_,origin);
    ROS_INFO_STREAM("map origin : "<<md_.local_bound_min_(0)<<","<<md_.local_bound_min_(1)<<","<<md_.local_bound_min_(2));
    map_2d.info.origin.position.x = origin(0);
    map_2d.info.origin.position.y = origin(1);
    for(int x = md_.local_bound_min_(0);x<md_.local_bound_max_(0);x++)
    {
        for(int y = md_.local_bound_min_(1);y<md_.local_bound_max_(1);y++)
        {
            if(md_.occupancy_buffer_inflate_2d_[toAddress2d(x,y)]==1)
            {
                map_2d.data[(y-md_.local_bound_min_(1))*map_2d.info.width+(x-md_.local_bound_min_(0))]=100;
            }

        }
    }
    map_2d_pub_.publish(map_2d);
}


bool GridMap::odomValid() { return md_.has_odom_; }

Eigen::Vector3d GridMap::getOrigin() { return mp_.map_origin_; }

 int GridMap::getVoxelNum() {
   return mp_.map_voxel_num_[0] * mp_.map_voxel_num_[1] * mp_.map_voxel_num_[2];
 }

void GridMap::getRegion(Eigen::Vector3d &ori, Eigen::Vector3d &size)
{
  ori = mp_.map_origin_, size = mp_.map_size_;
}

void GridMap::inputPointCloud(const pcl::PointCloud<pcl::PointXYZ>& points, const int& point_num,
  const Eigen::Vector3d& robot_pos) {
  if (point_num == 0) return;

  Eigen::Vector3d update_min = robot_pos;
  Eigen::Vector3d update_max = robot_pos;

  Eigen::Vector3d pt_w, tmp;
  Eigen::Vector3i idx;
  int vox_adr;
  double length;
  double current_time_sec = ros::Time::now().toSec();
  for (int i = 0; i < point_num; ++i) {
    auto& pt = points.points[i];
    pt_w << pt.x, pt.y, pt.z;
    int tmp_flag;
    // Set flag for projected point
    if (!isInMap(pt_w)) {
      //* 若不在地图内，获取地图内的点
      // Find closest point in map and set free
      pt_w = closetPointInMap(pt_w, robot_pos);
      length = (pt_w - robot_pos).norm();
      if (length > mp_.max_ray_length_)
        pt_w = (pt_w - robot_pos) / length * mp_.max_ray_length_ + robot_pos;
      tmp_flag = 0;
    } else {
      length = (pt_w - robot_pos).norm();
      if (length > mp_.max_ray_length_) {
        pt_w = (pt_w - robot_pos) / length * mp_.max_ray_length_ + robot_pos;
        tmp_flag = 0;
      } else
        tmp_flag = 1;
    }
    posToIndex(pt_w, idx);
    vox_adr = toAddress(idx);
    md_.last_update_time_buffer_[vox_adr] = current_time_sec;
    SetCacheOccupancy(vox_adr, tmp_flag);

    for (int k = 0; k < 3; ++k) {
      update_min[k] = min(update_min[k], pt_w[k]);
      update_max[k] = max(update_max[k], pt_w[k]);
    }
    // Raycasting between camera center and point
    //* 遍历激光点和车体位置间线段，填充
    //! 由于是融合点云，不能是定位到pt的射线，而应是同一高度到点的射线
    Eigen::Vector3d robot_pose_valid(robot_pos[0], robot_pos[1], pt_w.z());
    double length_valid = (pt_w - robot_pose_valid).norm();
    Eigen::Vector3d pose_to_pt_dir = (pt_w - robot_pose_valid) / length_valid;
    int ray_step = std::ceil(length_valid / mp_.resolution_);
    for (int ray_step_index=1; ray_step_index<ray_step; ++ray_step_index) {
      Eigen::Vector3d pt_step = robot_pose_valid + pose_to_pt_dir*ray_step_index*mp_.resolution_;
      Eigen::Vector3i pt_step_idx;
      posToIndex(pt_step, pt_step_idx);
      boundIndex(pt_step_idx);
      int step_vox_adr = toAddress(pt_step_idx);
      SetCacheOccupancy(step_vox_adr, 0);
    }
  }

  // Bounding box for subsequent updating and overall updateing
  // for (int k = 0; k < 3; ++k) {
  //   md_->update_min_[k] = min(update_min[k], md_->update_min_[k]);
  //   md_->update_max_[k] = max(update_max[k], md_->update_max_[k]);
  // }

  // Update map data
  while (!md_.cache_voxel_int_.empty()) {
    int adr = md_.cache_voxel_int_.front();
    md_.cache_voxel_int_.pop();

    // FIXME Why does this happen?
    if (adr >= md_.count_hit_.size()) continue;

    double log_odds_update =
        md_.count_hit_[adr] >= md_.count_miss_[adr] ? mp_.prob_hit_log_ : mp_.prob_miss_log_;

    md_.count_hit_[adr] = md_.count_miss_[adr] = 0;

    if (md_.occupancy_buffer_[adr] < mp_.clamp_min_log_ - 1e-3) {
      md_.occupancy_buffer_[adr] = mp_.min_occupancy_log_;
    }

    md_.occupancy_buffer_[adr] =
        std::min(std::max(md_.occupancy_buffer_[adr] + log_odds_update, mp_.clamp_min_log_),
            mp_.clamp_max_log_);
  }
}

void GridMap::ClearAndInflateLocalMap() {
  //* 1. 膨胀参数占的格子
  int inf_step = ceil(mp_.obstacles_inflation_ / mp_.resolution_);
  //* 2. 清空原来的膨胀buffer
  for (int x = md_.local_bound_min_(0); x <= md_.local_bound_max_(0); ++x)
    for (int y = md_.local_bound_min_(1); y <= md_.local_bound_max_(1); ++y) {
      //* 清空2D buffer
      md_.occupancy_buffer_inflate_2d_[toAddress2d(x, y)] = 0;
      //* 清空3D buffer
      for (int z = md_.local_bound_min_(2); z <= md_.local_bound_max_(2); ++z) {
        md_.occupancy_buffer_inflate_[toAddress(x, y, z)] = 0;
      }
    }
  //* 3. 根据正常的buffer，由膨胀格子步长，进行膨胀
  for (int x = md_.local_bound_min_(0); x <= md_.local_bound_max_(0); ++x)
    for (int y = md_.local_bound_min_(1); y <= md_.local_bound_max_(1); ++y)
      for (int z = md_.local_bound_min_(2); z <= md_.local_bound_max_(2); ++z) {
        int id1 = toAddress(x, y, z);
        if (md_.occupancy_buffer_[id1] > mp_.min_occupancy_log_) {

          for (int inf_x = -inf_step; inf_x <= inf_step; ++inf_x)
            for (int inf_y = -inf_step; inf_y <= inf_step; ++inf_y) {
              //* 填充2D buffer
              Eigen::Vector2i inf_pt_2D(x + inf_x, y + inf_y);
              if (!isInMap(Eigen::Vector3i(inf_pt_2D[0], inf_pt_2D[1], z))) continue;
              int inf_adr_2D = toAddress2d(inf_pt_2D[0], inf_pt_2D[1]);
              md_.occupancy_buffer_inflate_2d_[inf_adr_2D] = 1;
              //* 填充3D buffer
              for (int inf_z = -inf_step; inf_z <= inf_step; ++inf_z) {
                Eigen::Vector3i inf_pt(x + inf_x, y + inf_y, z + inf_z);
                if (!isInMap(inf_pt)) continue;
                int inf_adr = toAddress(inf_pt);
                md_.occupancy_buffer_inflate_[inf_adr] = 1;
              }
            }
        }
      }
}

void GridMap::PublishVisualization() {
  std::string frame_id_ = "map";
  pcl::PointXYZ pt;
  pcl::PointCloud<pcl::PointXYZ> cloud;
  pcl::PointCloud<pcl::PointXYZ> cloud2;
  pcl::PointCloud<pcl::PointXYZ> cloud3;
  // 仅遍历当前更新空间的体素
  Eigen::Vector3i min_cut = md_.local_bound_min_;
  Eigen::Vector3i max_cut = md_.local_bound_max_;
  boundIndex(min_cut);
  boundIndex(max_cut);

  // for (int z = min_cut(2); z <= max_cut(2); ++z)
  for (int x = min_cut(0); x <= max_cut(0); ++x)
    for (int y = min_cut(1); y <= max_cut(1); ++y) {
      //* 2D inflate buffer
      if (md_.occupancy_buffer_inflate_2d_[toAddress2d(x, y)] == 1) {
        // Inflated occupied cells
        Eigen::Vector3d pos;
        indexToPos2D(Eigen::Vector2i(x, y), pos);

        pt.x = pos(0);
        pt.y = pos(1);
        pt.z = pos(2);
        cloud3.push_back(pt);
      }
      for (int z = min_cut(2); z <= max_cut(2); ++z) {
        if (md_.occupancy_buffer_[toAddress(x, y, z)] >
            mp_.min_occupancy_log_) {
          // Occupied cells
          Eigen::Vector3d pos;
          indexToPos(Eigen::Vector3i(x, y, z), pos);

          pt.x = pos(0);
          pt.y = pos(1);
          pt.z = pos(2);
          cloud.push_back(pt);
        }

        if (map_local_inflate_pub_.getNumSubscribers() > 0) {
          if (md_.occupancy_buffer_inflate_[toAddress(x, y, z)] == 1) {
            // Inflated occupied cells
            Eigen::Vector3d pos;
            indexToPos(Eigen::Vector3i(x, y, z), pos);

            pt.x = pos(0);
            pt.y = pos(1);
            pt.z = pos(2);
            cloud2.push_back(pt);
          }
        }
      }
    }

  cloud.width = cloud.points.size();
  cloud.height = 1;
  cloud.is_dense = true;
  cloud.header.frame_id = frame_id_;
  cloud2.width = cloud2.points.size();
  cloud2.height = 1;
  cloud2.is_dense = true;
  cloud2.header.frame_id = frame_id_;
  cloud3.width = cloud3.points.size();
  cloud3.height = 1;
  cloud3.is_dense = true;
  cloud3.header.frame_id = frame_id_;
  sensor_msgs::PointCloud2 cloud_msg;

  pcl::toROSMsg(cloud, cloud_msg);
  map_local_pub_.publish(cloud_msg);
  pcl::toROSMsg(cloud2, cloud_msg);
  map_local_inflate_pub_.publish(cloud_msg);
  pcl::toROSMsg(cloud3, cloud_msg);
  map_local_2D_pub_.publish(cloud_msg);
}


void GridMap::TimeDecayCallback(const ros::TimerEvent& /*event*/) {
  if(!md_.has_odom_) return;
  ClearAndInflateLocalMap();
 // 添加时间衰减
 ros::Time current_time = ros::Time::now();
 double current_time_sec = current_time.toSec();
 
 for (int x = md_.local_bound_min_(0); x <= md_.local_bound_max_(0); ++x)
   for (int y = md_.local_bound_min_(1); y <= md_.local_bound_max_(1); ++y)
     for (int z = md_.local_bound_min_(2); z <= md_.local_bound_max_(2); ++z)
     {
       int idx = toAddress(x, y, z);
       
       // 只处理占据的栅格
       if (md_.occupancy_buffer_[idx] > mp_.min_occupancy_log_)
       {
         // 如果长时间未更新，则应用衰减
         double time_since_update = current_time_sec - md_.last_update_time_buffer_[idx];
         if (time_since_update > md_.decay_time_threshold_)
         {
            // std::cout << "before " << md_.occupancy_buffer_[idx];
           // 应用衰减因子，逐渐将占据概率降低到未知状态
           md_.occupancy_buffer_[idx] = std::max(
               md_.occupancy_buffer_[idx] * md_.decay_factor_, 
               mp_.min_occupancy_log_);
              //  std::cout << " after " << md_.occupancy_buffer_[idx] << std::endl;
         }
       }
     }
}

void GridMap::PublishMapBoxAndLocalUpdateBox() {
  if (box_pub_.getNumSubscribers() == 0) return;
  std::string frame_id_ = "map";
  Eigen::Vector3d min_pose, max_pose, cube_pos, cube_scale;
  visualization_msgs::Marker mk;
  getRegion(min_pose, max_pose);

  cube_pos = 0.5 * (min_pose + min_pose + max_pose);
  cube_scale = max_pose;
  mk.header.frame_id = frame_id_;
  mk.header.stamp = ros::Time::now();
  mk.type = visualization_msgs::Marker::CUBE;
  mk.action = visualization_msgs::Marker::ADD;
  mk.id = 0;
  mk.ns = "map_box";
  mk.pose.position.x = cube_pos(0);
  mk.pose.position.y = cube_pos(1);
  mk.pose.position.z = cube_pos(2);
  mk.scale.x = cube_scale(0);
  mk.scale.y = cube_scale(1);
  mk.scale.z = cube_scale(2);
  mk.color.a = 0.3;
  mk.color.r = 0.0;
  mk.color.g = 1.0;
  mk.color.b = 0.0;
  mk.pose.orientation.w = 1.0;
  mk.pose.orientation.x = 0.0;
  mk.pose.orientation.y = 0.0;
  mk.pose.orientation.z = 0.0;

  box_pub_.publish(mk);
  indexToPos(md_.local_bound_min_, min_pose);
  indexToPos(md_.local_bound_max_, max_pose);
  cube_pos = 0.5 * (min_pose + max_pose);
  cube_scale = max_pose - min_pose;
  mk.header.frame_id = frame_id_;
  mk.header.stamp = ros::Time::now();
  mk.type = visualization_msgs::Marker::CUBE;
  mk.action = visualization_msgs::Marker::ADD;
  mk.id = 0;
  mk.ns = "local_box";
  mk.pose.position.x = cube_pos(0);
  mk.pose.position.y = cube_pos(1);
  mk.pose.position.z = cube_pos(2);
  mk.scale.x = cube_scale(0);
  mk.scale.y = cube_scale(1);
  mk.scale.z = cube_scale(2);
  mk.color.a = 0.3;
  mk.color.r = 1.0;
  mk.color.g = 0.0;
  mk.color.b = 0.0;
  mk.pose.orientation.w = 1.0;
  mk.pose.orientation.x = 0.0;
  mk.pose.orientation.y = 0.0;
  mk.pose.orientation.z = 0.0;

  box_pub_.publish(mk);

}

void GridMap::ResetBuffer() {
  std::fill(md_.occupancy_buffer_.begin(), md_.occupancy_buffer_.end(), mp_.clamp_min_log_ - mp_.unknown_flag_);
  std::fill(md_.occupancy_buffer_inflate_.begin(), md_.occupancy_buffer_inflate_.end(), 0);
  std::fill(md_.last_update_time_buffer_.begin(), md_.last_update_time_buffer_.end(), ros::Time::now().toSec());
  std::fill(md_.occupancy_buffer_inflate_2d_.begin(), md_.occupancy_buffer_inflate_2d_.end(), 0);
  
  std::fill(md_.count_hit_and_miss_.begin(), md_.count_hit_and_miss_.end(), 0);
  std::fill(md_.count_hit_.begin(), md_.count_hit_.end(), 0);
  std::fill(md_.count_miss_.begin(), md_.count_miss_.end(), 0);
}

void GridMap::ChangeMapOrigin(const Eigen::Vector3d& new_origin) {
  // 1. Update map origin
  mp_.map_origin_ = new_origin - Eigen::Vector3d(mp_.map_size_(0)/2, mp_.map_size_(1)/2, 0.0);
    
  // 2. Update map boundaries based on the new origin
  mp_.map_min_boundary_ = mp_.map_origin_;
  mp_.map_max_boundary_ = mp_.map_origin_ + mp_.map_size_;

  // 3. Reset map buffers (reuse existing reset code)
  ResetBuffer();

  // 4. Reset local bound
  if (md_.has_odom_) {
    // If we have valid odometry, recalculate local bound based on new origin
    posToIndex(md_.camera_pos_ + mp_.local_update_range_, md_.local_bound_max_);
    posToIndex(md_.camera_pos_ - mp_.local_update_range_, md_.local_bound_min_);
    boundIndex(md_.local_bound_min_);
    boundIndex(md_.local_bound_max_);
  } else {
    // Otherwise set default bounds
    md_.local_bound_min_ = Eigen::Vector3i(0, 0, 0);
    md_.local_bound_max_ = mp_.map_voxel_num_ - Eigen::Vector3i(1, 1, 1);
  }
}

bool GridMap::CheckAndUpdateMapOrigin(double margin_percentage) {
  if (!md_.has_odom_) {
    return false; // 没有有效的里程计数据，无法更新
  }

  // 计算边缘距离（以地图大小的百分比）
  Eigen::Vector3d margin = mp_.map_size_ * margin_percentage;
  
  // 获取当前机器人位置
  Eigen::Vector3d robot_pos = md_.camera_pos_;
  
  // 计算到边界的距离
  Eigen::Vector3d dist_to_min = robot_pos - mp_.map_min_boundary_;
  Eigen::Vector3d dist_to_max = mp_.map_max_boundary_ - robot_pos;
  
  // 初始化标志和偏移向量
  bool need_update = false;
  Eigen::Vector3d shift_vec(0, 0, 0);
  
  // 分别检查x和y方向（对于地面机器人，通常只关心水平方向）
  for (int i = 0; i < 2; ++i) {
    if (dist_to_min(i) < margin(i)) {
      // 太接近最小边界，向正方向移动
      shift_vec(i) = margin(i) - dist_to_min(i);
      need_update = true;
    } 
    else if (dist_to_max(i) < margin(i)) {
      // 太接近最大边界，向负方向移动
      shift_vec(i) = -(margin(i) - dist_to_max(i));
      need_update = true;
    }
  }
  
  if (!need_update) {
    return false; // 不需要更新
  }
  
  // 通过偏移当前原点来计算新的原点
  Eigen::Vector3d new_origin = mp_.map_origin_;
  new_origin(0) = std::floor(robot_pos(0));
  new_origin(1) = std::floor(robot_pos(1));
  
  // 更改地图原点
  ChangeMapOrigin(new_origin);
  
  return true; // 地图原点已更改
}

// GridMap
